#include <unistd.h>
#include <sys/time.h>
#include <sys/statfs.h>
#include <termio.h>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/char.hpp"

using std::placeholders::_1;
using namespace std::chrono_literals;

void monitorKeyboard(char *key)
{
    while (rclcpp::ok())
    {
        termios new_settings;
        termios stored_settings;
        tcgetattr(0, &stored_settings);
        new_settings = stored_settings;
        new_settings.c_lflag &= (~ICANON);
        new_settings.c_cc[VTIME] = 0;
        tcgetattr(0, &stored_settings);
        new_settings.c_cc[VMIN] = 1;
        tcsetattr(0, TCSANOW, &new_settings);
        *key = getchar();
        tcsetattr(0, TCSANOW, &stored_settings);
    }
}

class SendGoalTest : public rclcpp::Node
{   
public:

    char c = 'f';
    rclcpp::Publisher<std_msgs::msg::Char>::SharedPtr keyPublisher;
    rclcpp::TimerBase::SharedPtr keyPublishTimer;

    SendGoalTest(std::string name) : Node(name)
    {
        std::thread keyThread(monitorKeyboard, &c);
        keyThread.detach();

        keyPublisher = this->create_publisher<std_msgs::msg::Char>("my_key",1);
        keyPublishTimer = this->create_wall_timer(50ms,std::bind(&SendGoalTest::publishKey,this));

    }

    void publishKey()
    {
        if(c != 'f')
        {
            std_msgs::msg::Char key;
            key.data = c;
            keyPublisher->publish(key);
            c = 'f';
        }
    }


};

int main(int argc, char **argv)
{
   rclcpp::init(argc, argv);
    auto control_test = std::make_shared<SendGoalTest>("controltest");
    rclcpp::spin(control_test);
    rclcpp::shutdown();
    return 0;
}
